#include "M_MotionSolution.h"
M_MS ChassicMMS;

M_MS::M_MS()
{
    Vx=0;
    Vy=0;
    Vz=0;
	
    //左前轮为第一个，顺时针旋转
    memset(Motor_SpeedReference,0,sizeof(Motor_SpeedReference));



}

#define ONE_PI   (3.14159265)

float angle_to_radian = 0.01745f;//角度转弧度

double mx_sin(double rad)
{   
	double sine;
	if (rad < 0)
		sine = rad * (1.27323954f + 0.405284735f * rad);
	else
		sine = rad * (1.27323954f - 0.405284735f * rad);
	if (sine < 0)
		sine = sine*(-0.225f * (sine + 1) + 1);
	else
		sine = sine * (0.225f *( sine - 1) + 1);
	return sine;
}

double my_sin(double rad)
{
	int8_t flag = 1;
	
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		rad -= ONE_PI;
		flag = -1;
	}

	return mx_sin(rad) * flag;
}

double my_cos(double rad)
{
	int8_t flag = 1;
	rad += ONE_PI/2.0;
   
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		flag = -1;
		rad -= ONE_PI;
	}
	return my_sin(rad)*flag;
}


//麦轮解算(陀螺仪航向角参加计算,开启无头模式，即xy方向只与开机方向有关，与运动过程无关)
void M_MS::Task_Auto_Run(float V_x,float V_y,float V_z,float Imu_Yaw)
{
    //ang_motion 可以传陀螺仪的Z轴角速度
	float a,b,c,ang_motion=Imu_Yaw;
    Vx = V_x;
    Vy = V_y;
    Vz = V_z;
	// unsigned short int moto_num,motor_num,axis_num;
		

		//四轮麦轮
    a = (45 - ang_motion)*angle_to_radian;
    b = (45 + ang_motion)*angle_to_radian;
    c = a;
    // motor_num = 4;

	
	//合成公式
	Motor_SpeedReference[0] = +my_cos(a)*Vx + my_sin(a)*Vy - Vz;//1******2	 
	Motor_SpeedReference[1] = +my_cos(b)*Vx - my_sin(b)*Vy - Vz;//********
	Motor_SpeedReference[2] = -my_cos(c)*Vx - my_sin(c)*Vy - Vz;//********
	Motor_SpeedReference[3] = -my_cos(b)*Vx + my_sin(b)*Vy - Vz;//4******3	 	
}
//麦轮解算
void M_MS::Task_Auto_Run(float V_x,float V_y,float V_z)
{
    //ang_motion 可以传陀螺仪的Z轴角速度
	float a,b,c,ang_motion=0;
    Vx = V_x;
    Vy = V_y;
    Vz = V_z;
	// unsigned short int moto_num,motor_num,axis_num;
		

		//四轮麦轮
    a = (45 - ang_motion)*angle_to_radian;
    b = (45 + ang_motion)*angle_to_radian;
    c = a;
    // motor_num = 4;

	
	//合成公式
	Motor_SpeedReference[0] = +my_cos(a)*Vx + my_sin(a)*Vy - Vz;//1******2	 
	Motor_SpeedReference[1] = +my_cos(b)*Vx - my_sin(b)*Vy - Vz;//********
	Motor_SpeedReference[2] = -my_cos(c)*Vx - my_sin(c)*Vy - Vz;//********
	Motor_SpeedReference[3] = -my_cos(b)*Vx + my_sin(b)*Vy - Vz;//4******3	 	
}


//麦轮解算
void M_MS::Task_Auto_Run(float V_x,float V_y)
{
    //ang_motion 可以传陀螺仪的Z轴角速度
	float a,b,c,ang_motion=0;
    Vx = V_x;
    Vy = V_y;
    Vz = 0;
	// unsigned short int moto_num,motor_num,axis_num;
		

		//四轮麦轮
    a = (45 - ang_motion)*angle_to_radian;
    b = (45 + ang_motion)*angle_to_radian;
    c = a;
    // motor_num = 4;

	
	//合成公式
	Motor_SpeedReference[0] = +my_cos(a)*Vx + my_sin(a)*Vy - Vz;//1******2	 
	Motor_SpeedReference[1] = +my_cos(b)*Vx - my_sin(b)*Vy - Vz;//********
	Motor_SpeedReference[2] = -my_cos(c)*Vx - my_sin(c)*Vy - Vz;//********
	Motor_SpeedReference[3] = -my_cos(b)*Vx + my_sin(b)*Vy - Vz;//4******3	 	
}

